#include "Components/clock.h"
#include "Components/motor.h"
#include "Components/oled_hardware_i2c.h"
#include "Components/uart.h"
#include "ti_msp_dl_config.h"
#include <ti/driverlib/m0p/dl_interrupt.h>
#include "motion.h"
#include "line_patrol.h"
#include "servo.h"

#define TAG "Main"
char _str_all[128] = {0};

int main(void)
{
  SYSCFG_DL_init();
  SysTick_Init();

  uart_init();
  OLED_Init();
  motor_init();
  servo_init();

  OLED_ShowString(0, 0, (uint8_t *)"TI MSPM0 TEST", 16);
  OLED_ShowString(0, 2, (uint8_t *)"===Line Patrol===", 16);

  DL_GPIO_clearPins(LEDs_PORT, LEDs_LED_PIN);

  uart0_Log_str(TAG, "Init sucessfully!");

  // motion_speed_ctrl(150, 150);

  servo_set_angle(90);

  while (1)
  {
    // uint8_t patroler_code = get_line_patrol_code();
    int d = get_patroler_center_x();

    if (d < 0)
    {
      uart0_Log_str(TAG, "distanse is invalid! discard!");
      continue;
    }

    // patrol_turn_pool(d);

    sprintf(_str_all, "cur:%03d", d);
    // uart0_Log_str(TAG, _str_all);
    OLED_ShowString(0, 4, (uint8_t *)_str_all, 16);
  }
}
